//
//
//

package notorious.rascal.variometer.model;

import notorious.rascal.variometer.*;
import android.util.Log;

public class KalmanFilteredAltitude implements DataSource {
    public static double STANDARD_SEA_LEVEL_PRESSURE = 101325;  //in pascals
    private double mSeaLevelPressure;

    private double mRawPressure;
    private double mRawAltitude;
    private double mDampedAltitude;
    private double mKalmanAltitude;

    private boolean mDampedAltStarted = false;
    private String mName;

    private KalmanFilter mKalmanFilter;

    private KalmanFilteredVario mKalmanVario;
    private KalmanFilteredVario mDampedVario;
    
    private double mPositionNoise = 0.2;
    private double mAccelerationNoise = 1.0;
    private double mAltDamp = 0.05;


    public KalmanFilteredAltitude(double seaLevelPressure, String mName) {
        this.mSeaLevelPressure = seaLevelPressure;
        this.mName = mName;
        
        mKalmanFilter = new KalmanFilter(mAccelerationNoise);
        mKalmanVario = new KalmanFilteredVario(this, 1.0, KalmanFilteredVario.KALMAN_VARIO);
        mDampedVario = new KalmanFilteredVario(this, 0.05, KalmanFilteredVario.DAMPED_VARIO);
    }


    public synchronized double addPressure(double pressure, double time) {
        mRawPressure = pressure;
        mRawAltitude = 44330.0 * (1 - Math.pow((pressure / mSeaLevelPressure), 0.190295));

        mKalmanFilter.update(mRawAltitude, mPositionNoise, 0.04);

        mKalmanAltitude = mKalmanFilter.getXAbs();
        //Log.i("Vario", "Altitude = " + mKalmanAltitude);

        if (mDampedAltStarted) {
            mDampedAltitude = mDampedAltitude + mAltDamp * (mRawAltitude - mDampedAltitude);
        } else {
            mDampedAltitude = mRawAltitude;
            mDampedAltStarted = true;
        }

        mKalmanVario.addData(time);
        mDampedVario.addData(time);

        return mRawAltitude;
    }

    /**
     * Sets the altitude to a specific level in m and updates the sea level pressure. Returns the new
     * sea level pressure value.
     */
    public synchronized double setAltitude(double alt) {
        double qnh = mRawPressure / Math.pow(1.0 - (alt / 44330.0), 5.255);
        
        mRawAltitude = alt;
        mDampedAltitude = mRawAltitude;
        mDampedAltStarted = true;
        mSeaLevelPressure = qnh;

        return mSeaLevelPressure;

    }

    public double getSeaLevelPressure() {

        return mSeaLevelPressure;
    }

    public void setSeaLevelPressure(double seaLevelPressure) {
        this.mSeaLevelPressure = seaLevelPressure;
        mDampedAltStarted = false;
    }


    public double getRawPressure() {
        return mRawPressure;
    }

    public synchronized double getRawAltitude() {
        return mRawAltitude;
    }

    public synchronized double getDampedAltitude() {
//        Log.i("BFVAltitude", this.toString());
        return mDampedAltitude;
    }

    public boolean isDampedAltStarted() {
        return mDampedAltStarted;
    }

    public String getName() {
        return mName;
    }

    public KalmanFilteredVario getKalmanVario() {
        return mKalmanVario;
    }

    public KalmanFilteredVario getDampedVario() {
        return mDampedVario;
    }

    public double getVarioValue() {
        return mKalmanFilter.getXVel();
    }

    public double getPositionNoise() {
        return mPositionNoise;
    }

    public void setPositionNoise(double positionNoise) {
        this.mPositionNoise = positionNoise;
    }

    public String toString() {
        return "Alt:" + mName + ":" + mRawAltitude;
    }

    public synchronized double getValue() {
        return mDampedAltitude;
    }

    public double getAltDamp() {
        return mAltDamp;
    }

    public void setAltDamp(double mAltDamp) {
        this.mAltDamp = mAltDamp;
    }
}
